#include <public/caavoidobstacles_iface_v1.h>

// ***********************************************************************

// class EDROOMTopContext

// ***********************************************************************



	// CONSTRUCTORES***********************************************

CAAvoidObstacles::EDROOMTopContext::EDROOMTopContext (CAAvoidObstacles &act, TEDROOMBool & EDROOMpVarVObsDetected ):

	EDROOMcomponent(act)
	,Msg(EDROOMcomponent.Msg)
	,MsgBack(EDROOMcomponent.MsgBack)
	,AvoidObstaclesSysCtrl(EDROOMcomponent.AvoidObstaclesSysCtrl)
	,Timer(EDROOMcomponent.Timer)
	, VObsDetected ( EDROOMpVarVObsDetected )
 {

}

CAAvoidObstacles::EDROOMTopContext::EDROOMTopContext ( EDROOMTopContext &contex ):

	EDROOMcomponent(contex.EDROOMcomponent),
	Msg(contex.Msg),
	MsgBack(contex.MsgBack)
	,AvoidObstaclesSysCtrl(contex.AvoidObstaclesSysCtrl)
	,Timer(contex.Timer)
	, VObsDetected ( contex.VObsDetected )
 {

}

	// EDROOMSearchContextTrans********************************************

bool CAAvoidObstacles::EDROOMTopContext::EDROOMSearchContextTrans(TEDROOMTransId &edroomCurrentTrans){

	bool edroomValidMsg=false; 

	 switch(Msg->signal){

		 case ( EDROOMSignalDestroy ): 

			 edroomValidMsg=true;
			 edroomCurrentTrans.distanceToContext = 0 ;
			 edroomCurrentTrans.localId = -1 ;
			 break;

	}

	return(edroomValidMsg);

}

	// Component Methods  ****************************

void	CAAvoidObstacles::EDROOMTopContext::FCheckObstacles()

{
 
 print_char('.');
 
 VObsDetected=sensors_check_obstacles();
 
 
}



void	CAAvoidObstacles::EDROOMTopContext::FConfigOK()

{
 
		// reply 
	
	Msg->reply(SConfigOK);
 
 
 
 
}



void	CAAvoidObstacles::EDROOMTopContext::FHandleObstacle()

{
 print_string("\n\r\tOBST");
 actuators_halt();
 
		// send new Msg
	
	AvoidObstaclesSysCtrl.send(SObstacleDetected, EDROOMprioVeryHigh);
 
}



void	CAAvoidObstacles::EDROOMTopContext::FProgNextDetection()

{
 
		// Timing Service 
	 
			//Useful related methods
			 
			//Pr_Time	interval(Pr_Time::millis,int); // Declare time var with an initial value in ms
			//Pr_Time	interval(Pr_Time::micros,int); // Declare time var with an initial value in microseg
			//Pr_Time	interval(float); // Declare time var with an initial value in seconds
	 Pr_Time interval(0.2);
 
	Timer.InformIn( interval, EDROOMprioVeryHigh );
 
 
 
 
}



	//*****Pools



// ***********************************************************************

// class EDROOMTopState

// ***********************************************************************



	// CONSTRUCTOR*************************************************

CAAvoidObstacles::EDROOMTopState::EDROOMTopState (CAAvoidObstacles &act ):
	  EDROOMTopContext( act 
		, VObsDetected )
{

}

	// EDROOMBehaviour**********************************************

void CAAvoidObstacles::EDROOMTopState::EDROOMBehaviour(){

	TEDROOMTransId edroomCurrentTrans;

	edroomCurrentTrans = EDROOMIllegada();

	do{

		switch(edroomCurrentTrans.localId){

			case(Init):
				edroomNextState = Idle;
				break;
			case(Config):
				FConfigOK();
				edroomNextState = Idle;
				break;
			case(Start):
				edroomNextState = Ready;
				break;
			case(CheckObstacles):
				FCheckObstacles();
				if( VObsDetected ){
					FHandleObstacle();
					edroomCurrentTrans.localId = CheckObstacles_ObstDetected;
					edroomNextState = Ready;
				 } 
				else{
					edroomCurrentTrans.localId = CheckObstacles_NoObstacle;
					edroomNextState = Ready;
				 } 
				break;
		}

		switch(edroomNextState){

			case(I):
				edroomCurrentTrans=EDROOMIllegada();
				break;

			case(Idle):
				edroomCurrentTrans=EDROOMIdlellegada();
				break;

			case(Ready):
				FProgNextDetection();
				edroomCurrentTrans=EDROOMReadyllegada();
				break;

		}

		edroomCurrentState=edroomNextState;

	}while(Msg->signal != EDROOMSignalDestroy);

}



	// Inicio**********************************************

void CAAvoidObstacles::EDROOMTopState::EDROOMInicio(){

edroomCurrentState=I;

}



//	 ***********************************************************************

//	 llegada SubEstado I

//	 ***********************************************************************



TEDROOMTransId CAAvoidObstacles::EDROOMTopState::EDROOMIllegada(){

	TEDROOMTransId edroomCurrentTrans;

	edroomCurrentTrans.localId = Init;
	edroomCurrentTrans.distanceToContext = 0;
	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja Idle

	// ***********************************************************************



TEDROOMTransId CAAvoidObstacles::EDROOMTopState::EDROOMIdlellegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (SConfigAO): 

				 if ( *Msg->GetPInterface() == AvoidObstaclesSysCtrl) {

					edroomCurrentTrans.localId= Config;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

				break;

			case (SStart): 

				 if ( *Msg->GetPInterface() == AvoidObstaclesSysCtrl) {

					edroomCurrentTrans.localId= Start;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja Ready

	// ***********************************************************************



TEDROOMTransId CAAvoidObstacles::EDROOMTopState::EDROOMReadyllegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (EDROOMSignalTimeout): 

				 if ( *Msg->GetPInterface() == Timer) {

					edroomCurrentTrans.localId = CheckObstacles;
					edroomCurrentTrans.distanceToContext = 0 ;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	return(edroomCurrentTrans);

}



